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ABSTRACT 


Autonomous Underwater Vehicles (AUV) have been used to accomplish a_ 
variety of missions, and their roles are expanding as sensors, computers and 
algorithms became more sophisticated. Sensors and control algorithms are of primary 
importance in solving a target mtercept or a docking problem. Three control 
algorithms (pursuit guidance, proportional navigation and a linear quadratic regulator) 
and three target information scenarios (perfect target information, noisy range and 
angle measurements and noisy angle measurements) are investigated here. This thesis 
presents the results of a simulation study for an Anti-Torpedo Torpedo (ATT) 
intercept problem for three different target trajectories, three target information 
scenarios and three guidance algorithms. This simulation study also includes the 
docking of an AUV to a moving platform. The proportional navigation algorithm is 
the most accurate for use in the ATT intercept problem, always achieving the smallest 
muss distances. However, it is of little use in the docking problem because it cannot 
control relative velocities. The linear quadratic regulator is successfully used to dock 


an AUV to a moving platform. 
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I. INTRODUCTION 


A. BACKGROUND 

Autonomous Underwater Vehicles (AUV) have been used for some time to 
accomplish a variety of missions. This thesis will approach two of these missions, the intercept 
problem for an Anti-Torpedo Torpedo (ATT) and the docking problem for a more general 
AUV. For these problems, the guidance algorithm used and the quality of the target estimates 
are major factors in mission success. 

A general assumption made in the study of intercept weapons is that the interceptor 
has both speed and maneuverability advantages over the target. This is often true in a missile- 
versus-aircraft scenario but is false in the underwater ATT scenario. The simulation of the 
ATT intercept problem studied here will assume that both vehicles have similar speeds and 
similar maneuverability characteristics. Another limitation in the underwater ATT scenario 
is the target measurement delay caused by the sound propagation speed in salt water, a result 
of the almost universal use of acoustic sensors at intermediate and long ranges in the ocean, 
as compared to optical or electromagnetic measurements used in aerial intercepts. Acoustic 
underwater measurements are also less precise than electromagnetic ones. The nonlinearities 
of the conducting medium are more pronounced in salt water than in air, and sonars have, in 
general, less resolution that radars. To take all of this into account, the simulation process 
assumes the existence of large noise levels and time delays in the measurements. 

The ATT sensors will generally be similar to torpedo acoustic sensors measuring the 
line-of-sight angles to the target. Two different target information scenarios will be 
considered, the case where the ATT has an active sensor providing both range and angular 
information, and the case where the ATT has only passive sensors providing only angular 
information. Two different Extended Kalman Filters will be designed to process the noisy 
measurements. Also, two different target maneuvering scenarios will be studied in detail: a 
target in straight line motion at a constant speed and a target performing a zigzag maneuver 
at constant speed. A target in a sinusoidal motion pattern will also be included for some 


comparisons. 











The docking problem is inherently more difficult because it attempts to match both 
position and velocity coordinates of the docking vehicle with a fixed position (the dock) or 
(even more difficult) with a moving target. This thesis will examine the more difficult 
problem. 

The docking of vehicles is generally done in "manual mode". To perform a docking 
maneuver in "automatic mode" raises a series of problems that do not exist in the intercept 
problem. Most of the known guidance algorithms for use in weapons are designed only to 
minimize the position difference between the commanded vehicle and the target vehicle. For 
the docking problem there must also be a velocity and/or orientation match between the two 
vehicles or between the vehicle and the fixed dock. Because this is a very general problem, 
this thesis will try to solve one of the harder cases, the docking of an autonomous vehicle to 
a moving target. The simulation will also assume that the AUV has maneuverability 
limitations but has, for this specific case, a speed advantage over the target. Only with a speed 
advantage can the docking be accomplished, allowing the AUV to close on the target from 


the rear. 


B. OBJECTIVES 

The objectives of this thesis are to investigate the performance of control algorithms 
for the intercept and for the docking problems. The evaluation of control algorithms will be 
based on the miss distance between the ATT and the target. Different simulations will be 
made to find the influence of individual factors on the performance of each guidance law. The 
Kalman filter estimates will also be analyzed, and the feasibility of using the Kalman filter to 
process noisy angle-only measurements will be studied. In the docking problem, the objective 
is to derive a control algorithm that will work for arbitrary initial positions and velocities. 

The simulation will be done usmg MATLAB with SIMULINK. The simulation 
program is divided into a number of blocks performing very specific tasks. This way it will 


be easy to modify the simulation just by replacing individual M-Files or S-functions. 








c. THESIS OUTLINE 


The analysis, as well as the results obtained from simulations, will be presented in the 
five subsequent chapters. Chapter II describes the vehicle's equations of motion, a simulation 
overview and the two versions of Extended Kalman filter. In Chapter III, the control 
algorithms used in the simulation are presented. Chapter IV describes the setup for the 
intercept problem and presents the simulation results, and Chapter V does a similar job for 
the docking problem. Chapter VI summarizes the results and conclusions from the evaluation 


of both problems. Finally, the Matlab M-files and S-functions are presented in the Appendix. 








Il. MATHEMATICAL MODELS AND SIMULATION 


A. GENERAL 
This chapter describes the coordinate systems and the mathematical models used for 
the underwater vehicles, the measurement model for the sensors used in the AUV and the 


extended Kalman filter estimator. 


B. COORDINATE SYSTEMS 

The motion of vehicles is generally described using two different reference frames, an 
inertial reference frame in global coordinates, Figure 1, and a body fixed coordinate system 
with the vehicle's velocity vector aligned with the x-axis and orthogonal y and z axes 
corresponding to yaw and pitch planes, Figure 2:°This simulation will use a total of three 
reference frames: an inertial one that has its origin at the launch position of the Autonomous 
Underwater Vehicle, and two others with body-fixed coordinate axes aligned at all times with 
the individual vehicles, AUV and target. For the angular measures to be expressed with 
standard compass angles, the inertial reference axis has the x-axis pointing toward North, the 


y-axis pointing toward East and the z-axis pointing toward the zenith. Each body-fixed 





Rast 


Figure 1. Inertial reference frame. Figure 2. Body-fixed reference frame. 





coordinate frame has its x-axis aligned with the vehicle's longitudinal axis of symmetry, which 
is assumed to coincide with the direction of the vehicle's velocity vector. 

| Position of the body-fixed axis is expressed by the inertial position of its origin, and 
its orientation is defined by the Euler angles ¥, © and ®. Here is the angle between inertial 
and body-fixed x-axes measured in the horizontal plane, © is the angle between the horizontal 
and the body-fixed x-axes measured in the vertical plane, and ® is the angle between the 


horizontal and the body-fixed y-axes measured in the OYZ body-fixed plane. 


C. EQUATIONS OF MOTION 
The equations of motion used in the simulation are a set of general and nonlinear 
differential equations that relate the vehicle's velocities in body-fixed coordinates (u,v,w) and 
the Euler angles (¥, ©, ®) with the vehicle's velocities expressed in inertial coordinates (x=v,, 
y=v,, Z=V,): 


X=ucos ¥ cos @ 
+v(cos ¥ sm @ sin © - sin ¥ cos © ) 
+w(cos ¥ sn @cos ©+sin ¥ sin @ ) 
y=usin ¥ cos 8 


+v(sin ¥ sin @ sin ® + cos ¥ cos @ ) (2.1) 
+w(sin ¥ sin @cos ® -cos ¥ sm © ) 
Z=-usin® 


+vcos 8 sin Y 
+weos @cos ®, 


where u (surge), v (Sway) and w (heave) are the vehicle's velocities in body-centered 
coordinates. x, y, and Z are the vehicle's velocities in inertial coordinates. The Euler angular 
rates are functions of the preexisting Euler angles (¥, ©, ®) and the vehicle turn rates in 
body-fixed coordinates (p, 9,7): 


& -p+qsin ® tan @+rcos ® tan @ 
@=qgcos © -rsin © 
sin ® , £08 ® (2.2) 





where p, g and r are the respective Roll, Pitch and Yaw rates. All the vehicles’ trajectories are 


simulated using equations 2.1 and 2.2. Vehicle velocities (u(t), v(t), w(f)) and turn rates (p(), 





q(t), r(t)) are the results of control inputs to propellers, thrusters and/or steering planes. These 
control mechanisms and their dynamics are peculiar to each individual vehicle. In this study 
we assume v(f) = w(f) = 0. For the ATT study u(Z) is taken as a constant, corresponding to a 
constant speed vehicle. For the docking problem u(t) is assumed to be subject to linear drag 
when adjusting to the desired control input velocity: z(r) = -ku(t)+u, where wu, is the desired 
control input velocity. Other control inputs are the turn rates p, g and r. This model accurately 
simulates a vehicle such as a torpedo with an adjustable speed propeller or thruster system 
along the longitudinal body axis and control surfaces (steering planes) with negligible time 
constants for inducing turns. These assumptions are adequate for the proof of concept studies 
featured in this thesis, but more detail in modeling thrust and control surface dynamics would 


be required for a real vehicle simulation. 








D. SIMULATION 


Figure 3 represents a simplified block diagram of the code used in the simulation 


studies. 


Target Delay 





AUV 
Figure 3. Simulation Diagram 


The Target block simulates the motion of a target vehicle and outputs position and 
target velocity (given in inertial coordinates) in the stx dimensional target state, x, 

The Delay block takes into account the sound propagation speed to delay the target 
information arrival to own sensors. This delay is calculated using the distance between 


vehicles and the sound propagation speed m salt water. 





The Observation block receives the difference between AUV position and target 
delayed position and produces two angles, bearing and elevation, and a distance. The relative 
coordinates # = ¥,-¥, = [x y z]’are transformed into spherical observations by 


ie [,. 22,7? 


a = arctan > 


x (2.3) 
B = arctan 
x“4y 
Simulations employing passive sensors do not use the range information. 
After the measurements are produced, white noise is added to the observations. The 
noise variance 1s the same for both angle measurements. The AUV simulations use 


measurements with the following noise characteristics: 


Oo. = 10.0 meters ; G, = O, = 1.0 degree . 


The Kalman estimator, which is described below, estimates all the target states from 
the noisy observations. Two different Kalman filters were designed because of the use of 
range and bearing measurements in some scenarios and angle only measurements in others. 
The outputs of this block are the estimated target states relative to the AUV. 

The Guidance block calculates the required control inputs to the AUV. These control 
inputs are all given in the body-fixed coordinate system. 

The AUV block simulates the motion of the AUV given the control inputs. The 
outputs are the position and velocity of the AUV in inertial coordinates, given in the six 


dimensional state vector X.,. 


E. KALMAN ESTIMATOR 
The state differential equation for the target in the inertial reference system can be 


represented by the following linear model: 


x 00010 041~ 00 0 

y 00001011 00 0]/, 

Z 000001 000 

de ‘ uy |. (2.4) 
V 00000 04\ "x 10 0 

s uy 

¥, 000000], 010 

ve 10 0000 0 v. 001 


Therefore, for a non-maneuvering target, the linear system is given by 
H(kT,\ = Cx(kT,\+¥,, | (2.6) 
with w, and v, the plant and measurement noise respectively. The term w, also takes into 


account the fact that the target trajectory may not be linear. The state transition matrix, ®, 


is then 
100 At 0 O 
010 0 Af O 
001 0 0 At 
@ - (2.7) 
000 1 0 «9 
000 0 1 O 
000 0 0 1 
The real measurements (7, «,) are assumed to be unbiased with covariance matrix 
o 0 0 
R*=|0 of 0 |, (2.8) 
yd 
0 0 oa, 


where o, ,o, and o, are the standard deviations of the measurements. The measurements are 


preprocessed to give the pseudo measurements (x,y,z) with covariance matrix 


R,= G,R'G, , (2.9) 
and 


10 


a8, 8; 
Or oa 


og, Og, 
Cr aa 


Og, Og, 
Or oa 


evaluated at r(kT,), a(kT,), and B(k7.) where 


a 2b 2.a 2b 21 


2.2 
ra 
g(r,e,B) = Ee 2 
a*b a 2b 21 


ry? 


ry *h? 





with a = tana and b = tanB. 


With the preprocessed measurements, the C matrix is given by 


1 0 0 0 
C-=1|0 10 0 


001000 


1+b? 


00 
0 0 


| For this problem, the discrete covariance matrix is 


=x 0 O 

Q-10 2 0 

0 0 & 

where 

T°? 

oo 

x= q’ ; 

TT: 

2 


6x6 


? 


(2.10) 


(2.11) 


(2.12) 


(2.13) 


(2.14) 


and q is a variable parameter that weights the relative importance between the assumed linear 


model and the noisy observations. This parameter is used to model the ability of the target to 


1] 








maneuver. High values of g will enable the Kalman filter to follow the target during turns, 
while low values of g produce a smooth estimate. 
The discrete time Kalman filter can then be summarized by a prediction step: 
eal 2 Lak (2. 15) 
Pipe CPO 20 (2.16) 


where P is the state error covariance matrix, and an update across a measurement step: 


Ky = Prac : [CP aie T+ RY" (2.17) 

Xpajea = Seale? Ke LW(AT) - CX, aig) (2.18) 
T 

Praja = U-K gC] Pray l-K,C¥ + K,RK; ats 


The above Kalman filter description assumes the existence of range and angle 
measurements. When the only observations are angles, the target system is not totally 


observable. The target model was modified, and now the system states are angles and angular 


rates. 
0 0010{] « 0 0 
B 0001 B 001] #. 
Pe foe + . (2.20) 
o, 000 0j{ ®, 1 0 |/u, 
o, 000 0])o, 01 


The estimate is now restricted to angles and angular rates, which limits the number 
of control algorithms that can be used. However, the estimation process is similar to the one 


described above. 
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It, CONTROL ALGORITHMS 


A. GENERAL 

This chapter describes the three control laws used in the simulation, Pursuit Guidance, 
Proportional Navigation and a Linear Quadratic Regulator. The first two laws are designed 
to achieve a target interception, while the third one can be used in a more general way, as for 
example, to dock a vehicle to a moving platform. The control inputs are calculated as desired 


vehicle turn rates. Desired speed is also a control input for the docking problem. 


B. PURSUIT GUIDANCE 

The purpose of this guidance law is to point the AUV velocity vector at the target. 
The control inputs are calculated using the angular difterenees between the line-of-sight 
vector to the target and the AUV velocity vector. 

The relative target positions 7, - 7,- 7 are rotated to the AUV body fixed coordinate 








system by 
cos (YF )cos (8) sin(P )cos(@,) -sin(@,) 
P isos * -sin (¥ ) cos (¥) 0 er (3.1) 
cos (P sin (8) sin(F .)sin(@) cos (8) 
where 
Vy 
Y = arctan — | 
P ¥. (3.2) 
V, 
@ = arctan - ——— , 
V_2V 2 (3.3) 
Y ~g Va 
and Fina = [Xe Ya Za | - 


The target angles in the body centered horizontal and vertical planes are then 


LS 








- 2 2 2 (3.4) 
XY rb"? rb 


& = arctan ——~—_ (3.5) 


Zz 
> 2 2 
X BV *2 rb 


The resulting inputs are yaw and pitch rates, u, and u,, given by 


uy, = ka, Bee 
u_= Ko.» (3.6) 


where k, is a pursuit guidance constant. A value of £,=3 is used throughout the simulation. 
When the total calculated acceleration exceeds the maximum allowable input 


acceleration, they are reduced using the following equations: 


F 2 2 
if u = uy + u, > U vax 
Uu - 


uo= u — 

y y (3.7) 
U nae 

uo= u.——. 

Zz Zz 
u 


Cc. PROPORTIONAL NAVIGATION 

This control algorithm drives the AUV into a collision course with the target. This is 
accomplished when the rate of change of the line-of-sight vector is zero. 

Relative target position and velocity are rotated to the body centered coordinate 
system as in the previous example. The line-of-sight rate is defined by the cross product 
between the relative position and velocity vectors, given in AUV body-fixed coordinates. 
xP | 


r 
r|body — r|body 
ei ae (3.8) 


With @ = [1 0 0]’, the unit vector along the AUV velocity vector, the required 


direction for the control inputs is given by 
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0 
COs (vy) = aeee (3.9) 
Ww 


sin (Y) 


The resulting control inputs are then 


sf v 


k,, sin (7) 7, | WI 
| 


(3.10) 


where 4,,, is a proportional navigation constant set equal to 4 throughout the simulation, 7, is 
the relative velocity vector calculated above, which represents the closing speed between the 
two underwater vehicles, and @, is the AUV speed. 

Ifthe total command input exceeds the maximum allowable one, u, and uw, are reduced 


as before. 


D. LINEAR QUADRATIC REGULATOR 
To apply an Optimal Linear Control the plant equations need to be linearized. Using 
the relative positions in the body centered coordinate system and assuming an AUV constant 


speed, the system can be represented by 


~*~) 1000100]; 


0 0 

y 00001 04/9 0 0 
Z Z 

000001 00 {|2 
ae ; a | (3.11) 
ge 00000 0 és 0 0 u. 
ve 00000 0//¥%, 1 0 
v 00000 0]Iy 0 1 
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However, this linearization of the real system has degenerated into an uncontrollable 
system. To fix this, equations permitting a variable thrust input were employed. Hence, the 


following set of linearized state equations was used. 


x 000100/;% 00 0 

y 0000101149 00 0}f, 

Z 000001 - 00 0 

5 oh + ul. (3.12) 
ve 000000{| ": 10 0 

° u, 

¥, 00000 04}¥%, 01 0 

v 00000 0]/y 001 


where u, is the surge rate, u, is the centripetal acceleration in the XY plane given by vehicle 
speed times yaw rate, and uw, is the corresponding acceleration in the XZ plane given by 
vehicle speed times pitch rate. For the ATT intercept problem, wu, 1s zero. 


The objective of this control algorithm is to minimize a cost function defined by 
J= [?[2O"O20) +a" RIO | ae, (3.13) 
SI 


where QO and R are weighting matrices. Q is a symmetric and non-negative definite matrix that 
weights the relative importance of the states' deviation from zero. R is a symmetric, positive 
definite matrix that limits the control energy during the trajectory. 
Control inputs are then given by 
u(t) = -F(t)xX(t), (3.14) 
where F(t) is the optimal feedback gain 
F(t) = R°QOB'OK®, (3.15) 
and, in steady state, K(t) satisfies a matrix Ricatti equation. 
0 = K(t)+OO)-KOBORIODB'OKO+KOAD+A 7K). (3.16) 
This calculation is done using the discrete time equivalent system where the steady state F 
matrix is given by the MATLAB call, F =dlgr(®,A,Q,R), with the system now characterized 
by a discrete time state equation 


w{(k+1)T,) = ®(T,)e(eT,)+ A(T, )a(ET,). (3.17) 


16 








The choice of the weighting matrices O and R is done according to the specific 


situation requirements. 
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IV. THE ATT INTERCEPT PROBLEM 


A. GENERAL 

This chapter describes the simulation of the intercept problem, as well as the results 
of the evaluation of the estimation filters and control algorithms. The case studied was an 
Anti-Torpedo Torpedo intercept problem. Three target motion scenarios were used to 


evaluate the guidance algorithms, a straight line, a zigzag pattern and a sinusoidal pattern. 


B. ESTIMATOR PERFORMANCE | 

Since the true target information is not available, an observer is needed to estimate the 
true target position and velocity or the true angles and angular rates. Gaussian white noise 
with o = 10.0 meters ; Oo, = o, = 1.0° is added to the observations. Since the sensors 
are placed in the interceptor vehicle, the range error is limited to 10% of the actual distance 
between target and interceptor for ranges less than 100 meters. To improve the filter 
performance at reduced range, the measurement covariance matrix used in the filter 1S 
adjusted for a range error 6, = 1.0 meters when the estimated range is less than 10 meters. 
The Kaiman filter for angle only measurements suffers no alterations throughout the 
simulation. 

Figures 4, 5 and 6 show examples of the estimated and true positions in the XY plane 
for each simulated target trajectory. These plots depict the typical estimation problems 
encountered when observing maneuvering targets. When the trajectory is not linear the 
estimation error increases considerably. As long as there is a long enough linear pattern after 
each turn, the estimates will converge to the true positions. The estimates of the sinusoidal 
target trajectory were not used as input for any guidance algorithm because of the large 
estimation errors. In that case the position errors were large and the velocity errors even 
larger, making the estimates of little use for any guidance law. Figures 7, 8, and 9 show the 


difference between estimated and real target positions, while Figures 10, 11 and 12 show the 


same difference for the velocities. 
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Figure 4. Actual and estimated target trajectory with no target maneuver. 
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Figure 5. Actual and estimated target trajectory with zigzag target maneuver. 
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Figure 6. 


Figure 7. 
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Actual and estimated target trajectory with sinusoidal target maneuver. 
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Errors in position estimates with no target maneuver. 
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Figure 8. 


Figure 9. 
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Errors in position estimates with zigzag target maneuver. 
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Errors in position estimates with sinusoidal target maneuver. 
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Figure 10. Errors in velocity estimates with no target maneuver. 
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Figure 11. Errors in velocity estimates with zigzag target maneuver. 
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Vx => (-.) : Vy=>(—-) ; Vz=>(..) 
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Figure 12. Errors in velocity estimates with sinusoidal target maneuver. 


When only angle measurements are used, the Kalman filter is modified to estimate 
angle and angular rates. Figures 13, 14, and 15 show the actual and estimated line-of-sight 
angles while Figures 16, 17 and 18 show the actual and estimated angular rates for all three 
target trajectories. Because of the difficulty of estimating those parameters with an assumed 
linear model for a very nonlinear system and angle only observations, the simulation 
assumes that the estimator has the ability of producing numerical derivatives of the noisy 
angle measurements. Although this technique introduces very noisy pseudo angular rate 


measurements, the filter performance is, in general, improved. 
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Figure 13. Actual and estimated line-of-sight angles with no target maneuver. 
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Figure 14. Actual and estimated line-of-sight angles with zigzag target maneuver. 
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Actual and estimated line-of-sight angles with sinusoidal target maneuver. 
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Errors in line-of-sight rate estimates with no target maneuver. 
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Horizontal rate =>(..) ; Vertical rate => (-.) 
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Figure 17. Errors 1n line-of-sight rate estimates with zigzag target maneuver. 
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Figure 18. Errors in line-of-sight rate estimates with sinusoidal target maneuver. 
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C. ALGORITHMS PERFORMANCE 


To evaluate the control laws by themselves, independently of estimation errors, the 
simulation was also run with perfect target information. Perfect target information assumes 
that there is no delay in the target information. However, the delay due to the low 
propagation speed of sound proved to be of negligible significance. The sensors are placed 
‘on the ATT and, as the range difference between the vehicles decreases so does the 
information delay, ending up being negligible for short distances. Figures 19 through 27 show 
the trajectories of target and ATT for the three target trajectories and the three control laws. 


Also shown is the minimum miss distance for each case. 
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Figure 19. ATT and target trajectories with pursuit guidance. 
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Miss Distance of 0.35 meters 
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Figure 20. ATT and target trajectories with proportional navigation. 
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Figure 21. ATT and target trajectories with linear quadratic regulator. 
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Figure 22. ATT and target trajectories with pursuit guidance. 


Miss Distance of 0.33 meters 


400 


200 


X-Axis (m) 


~400 





500 1000 
Y-Axis (m) 


Figure 23. ATT and target trajectories with proportional navigation. 
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Miss Distance of 3.67 meters 
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Figure 24. ATT and target trajectories with linear quadratic regulator. 
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Figure 25. ATT and target trajectories with pursuit guidance. 
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Miss Distance of 0.39 meters 
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Figure 26. ATT and target trajectories with proportional navigation. 
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Figure 27. ATT and target trajectories with linear quadratic regulator. 
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The proportional navigation control algorithm is the one that produces the best 
results. The evaluation of the algorithm is based upon the miss distance. Even for the case 
where the target is performing a sinusoidal pattern, the miss distance is only 0.39 meters. 

The analysis of the problem needs to take all factors into account. Simulations were 
made for noisy range and angle observations, and for noisy angle observations. Because of 
the poor Kalman filter estimates for the sinusoidal target maneuver, the miss distances were 
very large for all guidance laws, and results are not presented for that case (some simulations 
achieved miss distances of more than 50 meters). Also, because the linear quadratic regulator 
requires an estimate of all the system states, it was not used with angle only measurements. 
_As seen in Chapter IIL, section E, the estimates are angle and angular rates instead of position 
and velocity. 

For each different case, ten simulation runs were generated. Although ten runs cannot 
be considered large enough for the generation of important statistical conclusions, they are 
enough to evaluate the performance of the guidance laws. A summary of all results is shown 
on Tables 1, 2 and 3. 


= RANGE & ANGLE ANGLE 


PORSUT 
unqu[am [oe |. | - [16 


Table 1. Miss Distances for a target in a straight line. 
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| RANGE@ ANGLE | ANGLE | TRUE | 
ee ee ee ee 
-rursu| 1osi_| 017 | 98 | 020 | 930 
ppropnav| 120 | os2 | 126 | oat | 033 
fumouap| sas [om | - | - | 267 _ 


Table 2. Miss as for a cat in a zigzag pattern. 
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Table 3. Miss Distances for a target in a sinusoidal pattern. 


Again, the proportional navigation algorithm is the one with the best performance. In 
all the cases studied, the miss distances were small enough to predict a hit. The miss distance 
is evaluated between the centers of mass of both vehicles, and, miss distances of the order of 
the ones obtained correspond to hits. The linear quadratic regulator also has a small miss 
distance for a non-maneuvering target. It is important to remember that the poor results 
obtained for some algorithms were caused by the limitations imposed on the simulation, the 
similar speeds for both target torpedo and ATT, and the similar maneuverability 
characteristics. The maximum angular rate for the ATT was set to 0.5 radians per second 


( approximately 30 degrees per second ). 
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V. THE DOCKING PROBLEM 


A. GENERAL 


This chapter describes the simulation of the docking problem and the control strategy 


found to solve it. The major difference between this problem and the previous one is that the 
objective is now to match position and velocity of both vehicles. Hence, this simulation 
addresses a harder part of the general docking problem: instead of docking to a fixed 
platform, the autonomous vehicle will dock to a moving target. The target, however, will 
move in a straight line at a constant speed. In order to solve the docking problem 
independently of the initial vehicle's position and velocity, the AUV has a speed advantage 
over the target. The limits on AUV maneuverability remain the same. 

All the simulation procedures were modified to take into account the ability of the 
AUV to change speed. The guidance algorithm now calculates the desired angular rates and 
the desired vehicle speed. The equations of motion remain the same, but the AUV 
longitudinal speed is now an input. To simulate a first order drag term, the vehicle output 


speed is given by 





(5.1) 


where & is a constant coefficient. 


B. DOCKING METHOD 

The obvious guidance algorithm to be chosen, from the three mentioned in Chapter 
III, is the Linear Quadratic Regulator. This algorithm allows the minimization of the 
difference of both position and velocity between vehicles. The calculation of the control 
inputs is done separately for angular rates and speed. The angular rates are calculated 
according to equations 3.14 through 3.16, and the vehicle speed is another input. The 
minimization process is done by choosing the appropriate weighting matrices Q and R. 


However, the relative weights of position and velocity necessary to achieved a successful 
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docking are functions of the vehicles’ relative positions and velocities. The way to solve it was 
to separate the process into three different stages. 

The AUV initially closes to a position 10 meters astern of the target, aligned with the 
vehicles longitudinal axis and with a lateral separation of 20 meters. The lateral separation is 
chosen to be either to starboard or port of the target vehicle. During this initial approach, the 
ratio of position and velocity weights is equal to one. During this phase, the control inputs are 
calculated to move the AUV to the chosen position relative to the target and to align the 
AUV with the target longitudinal axis. This approach is done at a speed 2 m/s higher than the 
estimated target speed. The second step is to place the AUV at a position 5 meters alongside 
the target. The weights for position and velocity remain the same and the input speed is now 
equal to the estimated target speed plus 1 m/s. The third step is the final maneuver to 
accomplish docking. The position to velocity weight ratio is now 1/50, and the desired AUV 
speed is the estimated target speed plus 0.1 m/s. When the AUV is considered docked to the 
target, the AUV speed is reduced to the estimated target speed and the position to velocity 
weight ratio is changed to 1/500. | 

This docking process was simulated for a series of target initial positions and 
velocities. Figures 28 through 31 show several examples for different target initial conditions. 
In all the examples, two different AUV trajectories are shown, one for docking on each target 
side. The docking maneuver was effective in all cases for all target initial conditions and for 
both docking sides. The docking side is selected in the program that initializes the simulation 
process. Nevertheless, if the docking side is unimportant, the program can choose the one 
requiring the less control effort or the smaller time to docking. 

Initial simulation trials used a single range/bearing sensor with rather coarse range 
accuracy, and results were poor. Later trials used the sensor accuracy described in section B 
of Chapter IV . For these simulation runs it was found that the performance of the control 
algorithm was about the same for true target information and for the Kalman estimates from 
the noisy range and angle measurements. This happens because we assumed that the AUV 
employed two different kinds of sensors, a long range one for the initial approach, and a more 


accurate one for the final approach. 
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Figure 28. | Docking maneuver, example 1. 
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Figure 29. Docking maneuver, example 2. 
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Figure 30. Docking maneuver, example 3. 
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Figure 31. Docking maneuver, example 4. 
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VI. SUMMARY, CONCLUSIONS AND RECOMMENDATIONS 


This chapter summarizes all the simulation cases and presents thesis conclusions for 
sensors and guidance for the underwater intercept and docking problems. Some 


recommendations for follow-on work are also presented. 


A. SUMMARY 

The interception and docking problems were studied using different assumptions. For 
the ATT interception problem, the target was allowed to follow three distinct trajectories: a 
straight line, a zigzag pattern, and a sinusoidal pattern. Three target information scenarios 
were used: perfect target information (both position and velocity), noisy range and angle 
measurements, and noisy angle measurements. Due to the poor velocity estimates for a target 
performing a sinusoidal pattern, this target trajectory was studied only when perfect target 
information was used. Three control laws: pursuit guidance, proportional navigation, and a 
linear quadratic regulator, were tested for the interception problem. 

The docking problem was studied for a target moving in a straight line at constant 
speed, using both perfect target information and noisy range and angle measurements. The 


linear quadratic regulator was the control law used to maneuver the docking AUV. 


B. CONCLUSIONS 

The thrust of this thesis was to evaluate the performance of the Kalman estimators and 
the control algorithms. The extended Kalman filter closely tracks target position and velocity 
for the straight line and zigzag cases. When angle only measurements are used, the line-of- 
sight angles are estimated with small errors. The angular rate estimates, because the pseudo 
observations are numerical derivatives of the noisy angle measurements, have a larger error. 
The Kalman filter does not produce a good estimate for target velocity for the sinusoidal case, 


and the angular rate estimates are also poor. 
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The delay in the reception of target measurement had a negligible effect on the miss 
distances obtained. If the sensors were placed at a location other than on the interceptor, this 
effect should be considered. 

The proportional navigation algorithm achieves hits for all three target trajectories 
studied. Actually, the miss distances are approximately the same for all target trajectories. The 
linear quadratic regulator achieves reasonable miss distance for a target in a straight line, but 
its performance is poor in the other scenarios. The pursuit method is always bad. The reasons 
for the poor performance of the pursuit guidance are the speed and maneuverability 
restrictions imposed on the ATT. 

The linear quadratic regulator, because of its ability to minimize velocity differences, 
is an adequate guidance law to solve the docking problem. The solution for the docking 
problem is always achieved, independently of target initial position and velocity, and docking 


side, as long as there exists a speed advantage of the AUV over the target. 


C. RECOMMENDATIONS 

This study was done in a very general way. To be able to provide more answers on 
the feasibility of implementing the guidance algorithms on an actual vehicle, the dynamics of 
that specific vehicle should replace the general equations of motion used in this simulation. 
Once a real vehicle is simulated, the evaluation should be done in hit probability instead of 
average miss distance. For that, the calculation of the hit probability has to consider the 
dimensions of both target and interceptor vehicles. 

The existence of other measurements, like doppler readings, was not considered in this 
thesis. Finally, to decrease the estimation errors when the target is turning, a target maneuver 


tracking package can be implemented. 
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simplicity, the S-functions are presented as block diagrams. 


A. 





APPENDIX. MATLAB SOURCE CODE 


The M-files and S-functions used in the simulation are presented in this appendix. For 


TRUE TARGET INFORMATION INTERCEPT 


1. System Initialization 
clear 
global Ts u time2 KF 
A=[ 000100 ; 
000010 ; 
000001 ; 
000000 ; 
000000 ; 
000000 j; 
C=eye(3,6); 


B=[zeros(3);eye(3)]; 


D=zeros(6,3);% xdot()=Ax(t)+Bu(t) y@)=Cx(t)+Du(t) 


va=20;% att speed 

vt=20;% target speed 

% initial target Euler angles and position 
Y%phi0=0:theta0=elevation:psi0=heading 
Yphi0t=0;theta0t=.05;psi0t=-pi/2;% linear 
%xtO=50;yt0=1500;zt0=-100; 


Yphi0t=0;theta0t=.0S;psi0t=-5*pi/12;% zigzag 


%xt0=0;yt0=1500;ztO=- 100; 
phi0t0;theta0t=.05;psi0t=0;% sinusoidal 
xt0=0;yt0=1000;zt0=-100;% 

Yinitial ATT Euler angles and position 
phi0a=0;theta0a=0;psi0a=pi/2; 
xa0=0;ya0=0;za0=-12; 

Ts=.1;% sampling period 

u=[0;0];% initial control input 
tume2=0;% Controller counter 
tmax=42:% final time 


4] 





2, Simulation Diagram 
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Figure 32. True target information intercept simulation diagram. 
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Figure 33. Target dynamics. 
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4. ATT Dynamics 
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Figure 34. ATT dynamics. 
5. Pursuit Guidance 
function y=pursuit(x) 
% PURSUIT GUIDANCE ALGORITHM 
% 
% input: [ ATT heading and angle of site x(1:2); 
% relative target position and velocities in absolute coordinates _- x(3:8); 
% time x9); J 
¥, | 
% output: [ command accelerations in body centered coordinates y(1:2);  ] 
% 
global u Ts time2 
% calculations made rff [ time = n * Ts ] n=1,2,3.... 
if x(9)=time2 
tume2=x(9); 
fi=x(1); 
teta=x(2); 
% rotation matrix 
Lbi=[ _cos(fi)*cos(teta) sin(fi)*cos(teta) -sin(teta) 
-sin(fi) cos(fi) 0 


cos(fi)*si(teta) sin(fi)*sin(teta) cos(teta) ]; 
% rotate position to body centered coordinates 
rb=Lbi*[x(3);x(4);x(5)]: 
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alfay=atan2(rb(2),sqrt(rb(1)“24+1b(2)*2+16(3)2)); 
alfaz=atan2(rb(3),sqrt(rb(1)“2+1b(2)2+1b(3)%2)); 


kp=3; 
uay=kp*alfay; 
uaz=kp*alfaz; 
umax=.5; 
utot=sqrt(uay*2+uaz’2); 
if utot>umax 
uay=uay*umax/utot; 
uaz=uaz*umax/utot; 
end 
u=[uaz;uay]; 
end 
yu, 
6. Proportional Navigation 
function y=propnav(x) 
% PROPORTIONAL NAVIGATION ALGORITHM 
% 
% input: [ ATT heading and angle of site 
% relative target position and velocities in absolute coordinates 
% time 
% 
% output: | command accelerations in body centered coordinates 
% 
global u Ts time2 
% calculations made 1ff [ trme = n * Ts } n=1,2,3.... 
% 
if x(9)}~=time2 
time2=x(9); 
fi=x(1); 
teta=x(2); 
% rotation matrix | 
Lbi=[__—cos(fi)*cos(teta) sin(fi)*cos(teta) -sin(teta) 
-sin(fi) cos(fi) 0 


cos(fi)*sin(teta) sin(fi)*sin(teta) cos(teta) }; 
% rotate position and velocity to body centered coordinates 
rb=Lbi*[x(3):x(4);x(5)]; 
vb=Lbi*[x(6);x(7);x(8)]; 
wb=cross(rb,vb)/norm(rb)*2; 
ex=[1;0;0]; 
int=cross(wb,ex); 
ep=int/norm(int); 
kpn=4, 
uay=kpn*ep(2)*norm(wb)*norm(vb)/Ts/va; 
uaz=kpn*ep(3)*norm(wb)*norm(vbyTs/va; 
umax=0.5; 
utot=sqrt(uay*2+11az"2); 
if utot>umax 


44 














uay—uay*umax/utot; 


uaz=uaz* umax/utot; 
end 
u=[uaz;uay]; 
end 
y-u, 
fe Linear Quadratic Regulator 
function y=linquad(x) 
% LINEAR QUADRATIC REGULATOR ALGORITHM 
% 
% input: [ ATT heading and angle of site 
% relative target position and velocities in absolute coordinates 
% time 
% 
% output: [ command accelerations in body centered coordinates 
% 
global u Ts time2 KF 
% calculations made iff [ time = n * Ts j n=1,2,3.... 
% 
if x(9)}~=time2 
time2=x(9); 
fi=x(1); 
teta=x(2); 
% rotation matrix 
Lbi=[__cos(fi)*cos(teta) sin(fi)*cos(teta) -sin(teta) 
-sin(fi) cos(fi) 0 
cos(fi)*sin(teta) sin(fi)*sin(teta) _cos(teta) J; 
% rotate position and velocity to body centered coordinates 
rb=Lbi*[x(3);x(4):x(5)]; 
vb=Lbi* [x(6);x(7);x(8)]; 
ua=KF*[rb;vb]; 
uax=ua(1)/va; 
uay—ua(2)/va; 
uaz=ua(3 )/va; 
umax=.5; 
utot=sqrt(uay*2tuaz”2); 
if utot>umax 
uay=uay*umax/utot; 
uaz=uaz*umax/utot; 
end 
u=([uaz;uay]; 
end 
yu, 
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x(1:2); 
x(3:8); 
x(9); 


y(1:2); 


] 





RANGE AND ANGLE MEASUREMENTS INTERCEPT 
1. System Initialization 


clear 
global Ts F C RIH QP xhat u timel time2 KF 
A=[ 000100 ; 

O00010 ; 

000001 ; 

000000 ; 

000000 ; 

000000]; 
C=eye(3,6); 
B=[zeros(3);eye(3)]; 
D=zeros(6,3);% xdot()=Ax()+Bu(t) y=Cx@)+Du() 
va=20;% att speed 
vi=20;% target speed 
% initial target Euler angles and position 
Y%phi0=0:theta0=elevation:psi0=heading 
Yphidt=0;theta0t=.05;psi0t=-pi/2;% linear 
%xt0=50;yt0=1500;zt0=-100; 
Yphidt=0;theta0t=.05;psi0t=-5*pi/12;% zigzag 
%xtO=0;yt0=1500;zt0=- 100; 
phi0t=0;theta0t=.05;psi0t=0;% sinusoidal 
xt0=0;yt0=1000;zt0=-100;% 
%initial ATT Euler angles and position 
phi0a=0;theta0a=0;psi0a=p1/2; 
xa0=0;ya0=0;za0=-12; 
I=eye(6); 
Ts=.1:% sampling period 
[F,H]=c2d(A.B,Ts):% x(k+1)=F(ts)x(k)+H(Ts)utk) 
QQ=500*feye(3,6);zeros(3,6)];% Cost=integral (xQQxtuRRu)dt 
RR=eye(3); 
KF=dlqr,.H,QQ,RR);% feedback gain 
root=0;% white noise seed 
st=10;sa=1*(p1/180);sb=sa; 
R=[sr’2 0 0;0 sa%2 0;0 0 sb%2];% measurement noise covariance matrix 
qi 
SIG=[q*2*Ts%3/3 q*2*Ts*2/2;q*2*Ts%2/2 q2*Ts}; 
O=zeros(2); 
Q=[SIG O O;0 SIG O;0 O SIG];% _ discrete plant noise covariance matrix 
P=1e5*1;% initial state error covariance matrix 
xhat=[xt0;yt0;zt0;0;-vt;0];% initial Kalman estimate 
u=[0;0];% initial control input 
timei=0;% Kalmal filter counter 
time2=0:% Controller counter 
tmax=42;% final time 
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4, Noise Saturation 


function y=maxnoise(x) 
% NOISE REGULATOR 
% 
% input: { Noise x(1:3); 
% Target distance from ATT x(4); J 
% 
% output: [ min(x(1),x(4)/10);x(2);x(3) | 
% 
if (abs(x(1))>(x(4)/10)) 
dr=x(4)/10* sign(x(1)); 
else 
dr=x(1); 
end 
y=[dr;x(2):x3)]; 


5. Extended Kalman Filter 


function y=ekf_pseu(x) 

% DISCRETE TIME EXTENDED KALMAN FILTER 

% 

% mput: [ measurements  x(1i:3) ; (ange + bearing + elevation) 

% ATT position x(4:6) ; 

% ATT velocity x(7:9) ; 

% time x(10); ] 

% 

% output: [ relative estimated target position and velocities y(i:6); | 


% 
global Ts F CRI QP xhat timel 
xa=x(4:9);% ATT position and velocity 
% calculations made iff [ time =n * Ts ] n=1,2,3.... 
% 
if x(10)~=time 1 
time 1=x(10); 
xhat=F *xhat; 
P=F*P*F'+Q; 
1=x(1);a=x(2);b=x(3); 
rho=tan(a)*2*tan(b)*2+tan(a)*2+tan(b)*2+1 ; 
ta=tan(a);tb=tan(b)2; 
tal 2=ta*2+1 ;tb]2=tb*2+1; 





G=[ _ V/sqrt(tho) -r*ta*tal2*tb12/rho*(3/2) -1*tb*tal2*tb12/tho*(3/2) ; 
ta/sqrt(tho) —r*tal2*tb12/rho*(3/2) __—r*ta*tb*tal2*tb12/tho%(3/2); 


tb/sqrt(tb12) 0 r/sqrt(tb12) 
rhat=norm(xhat(1:3)-x(4:6)); 
if rhat<100 

Rd,1)=1; 
end 
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Rstar=G*R*G’; 
K=P*C'*inv(C*P*C'+Rstar); 
[2(1),z(2).2(3)}=sph2cart(a,b,r); 
yhat=C*(xhat-xa); 
xhat=xhat+K*(z'-yhat); 
P=(-K*C)*P*(-K*C)'+tK*Rstar*K'; 

end 

y=[xhat-xa]; 


ANGLE ONLY MEASUREMENTS INTERCEPT 


1. System Initialization 


clear 
global Ts F CRI QP xhat u timel time2 
A=[ 0010; 

0001; 

0000 ; 

0000]; 
C=eye(4); 
B=[zeros(2),eye(2)]; 
D=zeros(4,2):;% xdot()=Ax(t)+Bu(t) y@)=Cx(t)+Du(t) 
va=20;% att speed 
vt=20;% target speed 
% initial target Euler angles and position 
%phi0=0:theta0=elevation:psi0=heading 
Yph0t=0;theta0t=.05;psi0t=pi/2:% linear 
%xtO=50;yt0=1500;zt0=-100; 
Yphi0t=0;theta0t=.05;psi0t=-5*pi/12;% zigzag 
%xtO=0;yt0=1500;zt0=- 100; 
phi0t=0;theta0t=.05;psi0t=0;% sinusoidal 
xt0=0;yt0=1000;zt0=-100;% 
%initial ATT Euler angles and position 
phi0a=0;theta0a=0;psi0a=p1/2; 
xa0=0;ya0=0;za0=- 12; 
l=eye(4); 
Ts=.1;% sampling period 
[F,H]=c2d(A,B,Ts);% x(k+1)=F(ts)x(k)+H(Ts)u(k) 
root=0;% white noise seed 
_ ga=1*(p1/180);sb=sa; 
R=([sa“2 0 0 0;0 sb%2 0 0:00 5 0:0005]};% measurement noise covariance matrix 
q=1; | | 
SIG=[q*2*Ts%3/3 q*2*Ts%2/2;q*2*Ts*2/2 q*2*Ts]; 
O=zeros(2); 
Q=[SIG O;0 SIG];% discrete plant noise covariance matrix 
P=]e5*1I;% initial state error covariance matrix 
xhat=[atan2(-xtO,yt0);atan2(-zt0,sqrt(yt0“2+xt0”%2));0:0]:% initial Kalman estimate 
u=[0;0];% initial control input 
time1=0;% Kalmal filter counter 
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time2=0;% Controller counter 
tmax=42:% final time 


zs Simulation Diagram 
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Figure 37. Angle only measurements intercept simulation diagram. 
3. Kalman Filter 
function y=ekf_ao(x) 
% DISCRETE TIME EXTENDED KALMAN FILTER 
% 
% imput: [ measurements  x(1:2); (bearing + elevation) 
% x(3:4); (pseudo angular rates) 
x time x(5); j 
% 
% output: [ relative estimated target angles and angular rates y(1:2); | 
% 


global Ts FRIC QP xhat time] 
% calculations made iff [ time = n * Ts | n=1,2,3.... 


% 

if x(5)}-=timel 
time1=x(5); 
xhat=F *xhat; 
P=F*P*F'+Q; 
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K=P*C'*inv(C*P*C'+R); 
diff=x(1:4)-C*xhat; 
diff=asin(sin(diff)); 

xhat=xhat+K *diff; 
P=(1-K*C)*P*(-K*C)'+K*R*K". 


end 
y=[xhat}; 


4. Pursuit Guidance 


function y=pursuit(x) 

% PURSUIT GUIDANCE ALGORITHM ANGLE ONLY 

% 

% input: [ Observed bearing, elevation and rates of change (3:6); 

% time x(7); 

% 

% output: [ command accelerations in body centered coordinates (1:2); ] 

% 

global u Ts time2 

% calculations made iff [ time = n * Ts ] n=1,2,3.... 

% 

if x(7)}~=time2 
time2=x(7); 
fx(1);t=x(2);,a=x(3);b=x(4), 
px=cos(b)*cos(a);py=cos(b)*sin(a);pz=sin(b); 
alfay=atan2(-sin(f)*px+cos(f)*py,cos(f)*sin(t)*px+sin(f)*cos(f)*py-sin(t)*pz); 
alfaz= atan2(cos(f)*sin(t)*px+sin(f)*sin(t)*py+cos(t)*pz,cos(f)*sin(t)*px+ 

sin(f)*cos(f)*py-sin(t)*pz); | 


utot=sqrt(uay“2+uaz”2); 
if utot>umax 
uay=uay*umax/utot; 
uaz=uaz* umax/utot; 
end 
u=([uaz;uay]; 
end 


5. Proportional Navigation 


function y=propn_ao(x) 

% PROPORTIONAL NAVIGATION ALGORITHM ANGLE ONLY 

% 

% input: [ Observed bearing, elevation and rates of change x(3:6); 

% time x(7); J 


S] 


% 

% output: [ command accelerations in body centered coordinates y(1:2); ] 
% 

global u Ts time2 

% calculations made iff [ tume = n * Ts J n=1,2,3.... 

% 


if x(7)}~=time2 
time2=x(7); 
kpn=4; 
uay=x(5)*kpn; 
uaz=x(6)*kpn; 
umax=0.5; 
utot=sqrt(uay“2+uaz"2); 
if utot>umax 
uay=uay*umax/utot; 
uaz=uaz*umax/utot; 
end 
u=[uaz;uay]; 
end 
y-u, 
DOCKING PROBLEM 
1. System Initialization 
clear 


global Ts FR IH QP xhat u timel time2 KFO KF] KF2 vt vm dA dBi dB2 flag sr sa sb 
A=[ 000100 ; 

000010 ; 

O00001 ; 

000000 ; 

000000 ; 

000000}; 
C=eye(6); 
B=[zeros(3);eye(3) I; 
D=zeros(6,3);% xdot()=Ax(})+Bu(t)) yO=Cx(p+Du() 
vin=8;% max. ATT speed 
vt=5;% target speed 
%phi0=0:theta0=elevation:psi0=heading 


%phi0t=0;theta0t=0;psi0t=-pi/2; % #]} 
%xtO=100;ytO=200;zt0=- 12; 
%phi0t=0;theta0t=0;psi0=0; % #2 


%xtO=-50; yt0=100;zt0=- 12; 
“phidt=0;thetaOt=0;psi0t=p1/4; % #3 
%xtO=50;yt0=50;zt0=- 12; 

phi0t=0;thetaOt=—0;psi0t=-pi/2; % #4 
xt0=0-:yt0=100;zt0=-12; 

phi0a=0;theta0a=0;psi0a=pi/2;% initial ATT Euler angles 
xa0=0;ya0=0;za0=-12;% initial ATT positions 


a2 


I=eye(6); 

Ts=.1;% sampling period 

[F,H]=c2d(A,B,Ts);% x(k+1)=F(ts)x(k)+H(Ts)utk) 
QQ0=S500*eye(6);% Cost=integral (KQQx+uRRu)dt 
QQ1=[100* eye(3,6);zeros(3) 500*eye(3)}; 
QQ2=[eye(3,6);zeros(3) 500*eye(3)]; 

RR=0. 1] *eye(3); 

KFO=dlqr,H,QQ0,.RR);% feedback gain 

KF 1=dlqr(F,H,QQ1.RR); 

KF2=dlqr(F,H,QQ2,RR); 

root=0;% white noise seed 

st=5;sa=1*(pi/180);sb=sa; 

R=[sr“2 0 0;0 sa*2 0;0 0 sb*2];% measurement noise covariance matrix 
q=2;SIG=[q*2*Ts%3/3 q*2*Ts%2/2:q*2*Ts%2/2 q*2*Ts]}: 
O=zeros(2):; 

Q=[SIG O 0:0 SIG 0;0 O SIG];% discrete plant noise covariance matrix 
P=]e5*1;% initial state error covariance matrix 
xhat=[xt0;yt0;zt0;0;-vt;0];% initial Kalman estimate 
u=[0;0;0];% initial control input 

timel=0;% Kalmal filter counter 

time2=0;% Controller counter 

tmax=50;% final time 

dA=20;% distance behind 

dB1=-20;% 1st distance alongside 

dB2=-5;% 2nd distance alongside 

flag=0;% control for set point 
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Figure 38. Docking simulation diagram. 
3. Docking Algorithm 
function y=dock(x) 
% 
% DOCKING ALGORITHM 
% 
% input: [ AUV heading and angle of site 
% relative target position and velocities in absolute coordinates 
% time 
% 
% output: [ command accelerations in body centered coordinates 
% Desired AUV speed 
% 
global u Ts trme2 KFO KF1 KF2 vm vt dBi dB2 dA flag 
if x(15)}~=time2 
time2=x(15); 
fi=x(1); | 7 
teta=x(2); | 
Lbi=[ _cos(fi)*cos(teta) sin(fi)*cos(teta) -sin(teta) ; 





-sin(fi) cos(fi) 0 ; 
cos(fi)*sin(teta) sin(fi)*sin(teta) cos(teta) ]; 
if flaa—=0 





x(1:2); 
x(3:8); 
x(9); 


y(1:2); 
y(3); 


dist T=[x(6)/vt*dA;x(7)/vt*dA;0]+[x(7)/vt* dB 1;-x(6)/vt*dB1;0]; 


va=vtt2; 
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J 





end 





elseif flag] 
dist T=[x(7)/vt*dB2;-x(6)/vt*dB2:0}; 


va=vt+]; 
elseif flag—=2 

distT=0; 

va=vtt. 1; 
else 

distT=0; 

va=vt; 
end 


rbr=[x(3);x(4):x(5)]-[x@);x(10);x1 1]; 

vbr=[x(6);x(7):x(8)]-[x(12):x(13);x(14)]; 

if (nomn(rbr-distT)<3 & flag==0) 
flag=1; 

elseif (norm(rbr-distT)<1 & flag==1) 
flag=2; 

elseif (norm(rbr)<.2 & flag==2) 
flag=3, 

end 

if flag==0 | flag==1) 
KF=KFO; 

elseif flag==2 
KF=KF 1; 

else 
KF=KF2; 

end 

rb=Lbi*(rbr-distT); 

vb=Lbi* vbr; 

ua=KF*|[rb;vb]; 

uax=ua(1); 

uay=ua(2); 

uaz=ua(3); 

umax=20; 

utot=sqrt(uay*2+uaz”2); 

if utot>umax 
uay=uay*umax/utot; 
vaz=uaz*umax/utot; 

end 

u=[uaz;uay;va]; 


= 
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